#define CATCH_CONFIG_MAIN
#include <catch2/catch.hpp>

#include "common.h"
#include <Eigen/Geometry>

using namespace pcv;

TEST_CASE("Question 1", "Solve the projection using DLT approaches") {
    ProjectionData pdata;
    Matrix34d P_s = student_P(pdata.points3d, pdata.points2d);

    for (int i = 0; i < pdata.points3d.size(); ++i) {
        Vector2d uv = (P_s * pdata.points3d.at(i).homogeneous()).eval().hnormalized();
        REQUIRE((uv - pdata.points2d.at(i)).norm() == Approx(0.0).margin(0.01));
    }
}

TEST_CASE("Question 2", "Solve the perspective-n-point problem") {
    Matrix3d K;
    K << 800, 0, 400, 0, 800, 400, 0, 0, 1;

    ProjectionData pdata;

    Vector3d r_s, t_s, r_g, t_g;
    std::tie(r_s, t_s) = student_PnP(pdata.points3d, pdata.points2d, K);
    std::tie(r_g, t_g) = golden_PnP(pdata.points3d, pdata.points2d, K);

    REQUIRE((r_s - r_g).norm() == Approx(0.0).margin(1e-4));
    REQUIRE((t_s - t_g).norm() == Approx(0.0).margin(1e-4));
}
